#include "PID.h"
#include <stdio.h>
#include <stdlib.h>

PID_count *create_PID_COUNT_VALUE(int16_t instance_id) 
{
	PID_count *motor_PID = (PID_count *)malloc(sizeof(PID_count));

	return motor_PID;
}



float PID_COUNT(PID_count *pid, float actual_val)
{
	
	pid->Error = pid->want_count - actual_val;
	pid->integral += pid->Error;
	pid->output_val =   pid->Kp * pid->Error + 
						pid->Ki * pid->integral + 
						pid->Kd *(pid->Error -pid->LastError);
	pid-> LastError = pid->Error;
	
	return pid->output_val;
	
}

int PID_count_position(PID_count *pid, float actual_position_val)
{

	for(int i = 0 ; i <pid->want_position_count ; i++)
	{
		float position_count = i;
		PID_COUNT(pid, position_count);
		
	}
	pid->motorstop_flag = 1;
	return pid->motorstop_flag;
	
}
